from socket import timeout
import time
import 小车.舵机.serial as serial
import RPi.GPIO as GPIO
import atexit
import sys
import os

last_pwm_x=7.5
last_pwm_y=7.5
file_name="config.txt"
p_gpio_x=7
p_gpio_y=18
last_error_x=0

def check_data(data):
    if len(data)<12:
        return False
    if data[0] != '#':
        return False
    if data[1] != 'X':
        return False    
    if int(data[2:6])<=0 or int(data[2:6])>=900:
        return False
    if data[6] != 'Y':
        return False
    if int(data[7:11])<=0 or int(data[7:11])>=1300:
        return False
    if data[11] != '!':
        return False
    return True

def save_to_file(data):
    try:
        global file_name
        file=open(file_name,'w+')
        file.read()
        file.write(data)
        file.flush()
        file.close()
    finally:
        if file:
            file.close()
    return

def init_pwm_set():
    global last_pwm_x,last_pwm_y
    try:
        if not os.path.exists(file_name):
            return
        file=open(file_name,'r')
        data=file.readline()
        arr=data.split("&")
        if len(arr)==2:
            last_pwm_x=float(arr[0])
            last_pwm_y=float(arr[1])    
        file.close()
    except Exception as e:
        print(e)
        file.close()
    return


def send_pwm(p_gpio,last_pwm,target_pwm):
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BOARD)
    print('gpio',p_gpio,last_pwm,target_pwm)
    GPIO.setup(p_gpio,GPIO.OUT,initial=GPIO.LOW)
    pwm=GPIO.PWM(p_gpio,50)
    # pwm.stop()
    pwm.start(0)
    if last_pwm<target_pwm:
        while last_pwm<=target_pwm:
            pwm.ChangeDutyCycle(last_pwm)
            print('pwm',last_pwm)
            last_pwm=float(format(last_pwm+0.005,'.3f'))
            time.sleep(0.005)
    else:
        while last_pwm>=target_pwm:
            pwm.ChangeDutyCycle(last_pwm)
            print('pwm',last_pwm)
            last_pwm=float(format(last_pwm-0.005,'.3f'))
            time.sleep(0.005)
    time.sleep(0.02)
    pwm.stop()
    GPIO.cleanup()
    return last_pwm

def pid(last_pwm,x):
    if abs(x-400)<100:
        return 0
    # return float(format((x-400)*0.0015,'.3f'))
    global last_error_x
    Kp=-0.0003
    Ki=0.0015
    error_x=x-400
    dif_pwm_x=Kp*(error_x-last_error_x)+Ki*error_x
    last_error_x=error_x
    if last_pwm+dif_pwm_x<2.5:
        last_pwm=2.5
    elif last_pwm+dif_pwm_x>12.5:
        last_pwm=12.5
    else:
        last_pwm=last_pwm+dif_pwm_x
    return float(format(last_pwm,'.4f'))

def dif_pwm_y(y):
    if abs(y-640)<100:
        return 0
    return float(format((y-400)*0.0015,'.3f'))

def deal_data(data):
    if not check_data(data):
        return ""
    global last_pwm_x,last_pwm_y
    x=int(data[2:6])
    y=int(data[7:11])
    pwm_x=pid(last_pwm_x,x)
    pwm_y=last_pwm_y+dif_pwm_y(y)
    last_pwm_x=send_pwm(p_gpio_x,last_pwm_x,pwm_x)
    # last_pwm_y=send_pwm(p_gpio_y,last_pwm_y,pwm_y)
    save_to_file(str(last_pwm_x)+"&"+str(last_pwm_y))
    return data


def main():
    ser=serial.Serial(port="/dev/ttyAMA0", baudrate=9600,timeout=5)
    if not ser.isOpen():
        ser.open()
    init_pwm_set()
    ser.write("/dev/ttyAMA0 is open".encode("UTF-8"))
    data=str(last_pwm_x)+"&"+str(last_pwm_y)
    ser.write(data.encode("UTF-8"))
    atexit.register(GPIO.cleanup)
    try:
        while True:
            size=ser.inWaiting()
            if size==0:
                continue
            time.sleep(0.01)
            size=ser.inWaiting()
            receive=ser.read(size)
            print(receive)
            result=deal_data(receive)
            ser.flushInput()
            if len(result)>0:
                ser.flushOutput()
                ser.write(result.encode("UTF-8"))
                print("result",result)
            time.sleep(0.05)
    except Exception or KeyboardInterrupt as e:
        print('error',e)
        ser.close()

main()